{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Tutorial 01: Python Wrapper\n",
    "\n",
    "The collision checker provides you the functionality to check if basic geometric shapes and groups of shapes collide. Currently several basic shapes are available: axis-aligned rectangles (pycrcc.RectAABB), oriented rectangles (pycrcc.RectOBB), triangles (pycrcc.Triangle), circles (pycrcc.Circle), and polygons (pycrcc.Polygon). The most basic intersection test can be performed between these primitive shapes.\n",
    "\n",
    "## 1. Creating Basic Geometric Shapes\n",
    "\n",
    "We start with creating a set of basic shapes. We therefore need to import the Python wrapper ‘pycrcc’."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "import matplotlib.pyplot as plt\n",
    "import pycrcc\n",
    "from commonroad_cc.visualization.draw_dispatch import draw_object\n",
    "\n",
    "# Axis-aligned rectangle with width/2, height/2, x-position , y-position\n",
    "aabb = pycrcc.RectAABB(2.0, 3.0, 3.0, 2.0)\n",
    "\n",
    "# Oriented rectangle with width/2, height/2, orientation, x-position , y-position\n",
    "obb = pycrcc.RectOBB(1.0, 2.0, 0.3, 8.0, 10.0)\n",
    "\n",
    "# Circle with radius, x-position , y-position\n",
    "circ = pycrcc.Circle(2.5, 6.0, 7.0)\n",
    "\n",
    "# Triangle with vertices (x1, y1), (x2, y2), and (x3, y3)\n",
    "tri = pycrcc.Triangle(0.0, 0.0, 4.0, 0.0, 2.0, 2.0)\n",
    "\n",
    "plt.figure(figsize=(10, 10))\n",
    "draw_object(aabb, draw_params={'collision': {'facecolor': 'green'}})\n",
    "draw_object(obb, draw_params={'collision': {'facecolor': 'red'}})\n",
    "draw_object(circ, draw_params={'collision': {'facecolor': 'yellow'}})\n",
    "draw_object(tri, draw_params={'collision': {'facecolor': 'blue'}})\n",
    "plt.autoscale()\n",
    "plt.axis('equal')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In order to create a polygon, we need to define several components: the vertices of the outer boundary, the vertices of holes, and a triangle mesh which is used for collision checks. We can therefore use *Python Triangle*, which is a python wrapper around Jonathan Richard Shewchuk’s Triangle library."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import triangle\n",
    "\n",
    "# define the vertices of the outer boundary, we assume that we have no holes\n",
    "vertices = [[2.0, 0.0], [3.0, 0.0], [3.5, 1.5], [5.0, 2.0], [4.5, 2.5], [1.5, 1.5]]\n",
    "# triangulate the polygon\n",
    "number_of_vertices = len(vertices)\n",
    "segments = list(zip(range(0, number_of_vertices-1), range(1, number_of_vertices)))\n",
    "segments.append((0, number_of_vertices-1))\n",
    "triangles = triangle.triangulate({'vertices': vertices, 'segments': segments}, opts='pqS2.4')\n",
    "# convert all triangles to pycrcc.Triangle\n",
    "mesh = list()\n",
    "for t in triangles['triangles']:\n",
    "    v0 = triangles['vertices'][t[0]]\n",
    "    v1 = triangles['vertices'][t[1]]\n",
    "    v2 = triangles['vertices'][t[2]]\n",
    "    mesh.append(pycrcc.Triangle(v0[0], v0[1],\n",
    "                                v1[0], v1[1],\n",
    "                                v2[0], v2[1]))\n",
    "# create the polygon with the vertices of the outer boundary, the holes, and the triangle mesh\n",
    "polygon = pycrcc.Polygon(vertices, list(), mesh)\n",
    "\n",
    "# draw the polygon and its triangle mesh\n",
    "plt.figure(figsize=(10, 10))\n",
    "plt.subplot(211)\n",
    "draw_object(aabb, draw_params={'collision': {'facecolor': 'green'}})\n",
    "draw_object(obb, draw_params={'collision': {'facecolor': 'red'}})\n",
    "draw_object(circ, draw_params={'collision': {'facecolor': 'yellow'}})\n",
    "draw_object(tri, draw_params={'collision': {'facecolor': 'blue'}})\n",
    "draw_object(polygon, draw_params={'collision': {'facecolor': 'orange'}})\n",
    "plt.autoscale()\n",
    "plt.axis('equal')\n",
    "\n",
    "plt.subplot(212)\n",
    "draw_object(aabb, draw_params={'collision': {'facecolor': 'green'}})\n",
    "draw_object(obb, draw_params={'collision': {'facecolor': 'red'}})\n",
    "draw_object(circ, draw_params={'collision': {'facecolor': 'yellow'}})\n",
    "draw_object(tri, draw_params={'collision': {'facecolor': 'blue'}})\n",
    "draw_object(mesh, draw_params={'collision': {'facecolor': 'orange'}})\n",
    "plt.autoscale()\n",
    "plt.axis('equal')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2: Performing Collision Checks Between Basic Shapes \n",
    "\n",
    "Collision queries can be conducted using the function ‘collide’:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print('Collision between OBB and AABB: ', obb.collide(aabb))\n",
    "print('Collision between AABB and Circle: ', aabb.collide(circ))\n",
    "print('Collision between Circle and OBB:  ', circ.collide(obb))\n",
    "print('Collision between Triangle and AABB:  ', tri.collide(aabb))\n",
    "print('Collision between Polygon and Triangle: ', polygon.collide(tri))\n",
    "print('Collision between Polygon and Circle: ', polygon.collide(circ))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3: Creating Groups of Shapes\n",
    "\n",
    "Several basic shapes can be grouped into one collision object using the class ShapeGroup. Collision checks can be performed similiar to basic shapes."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# create two shape groups\n",
    "sg_1 = pycrcc.ShapeGroup()\n",
    "sg_1.add_shape(obb)\n",
    "sg_1.add_shape(aabb)\n",
    "\n",
    "sg_2 = pycrcc.ShapeGroup()\n",
    "sg_2.add_shape(circ)\n",
    "sg_2.add_shape(tri)\n",
    "\n",
    "print('Collision between Circle and Shapegroup 1: ', circ.collide(sg_1))\n",
    "print('Collision between Shapegroup 1 and Shapegroup 2: ', sg_1.collide(sg_2))\n",
    "\n",
    "plt.figure(figsize=(10, 10))\n",
    "draw_object(sg_1, draw_params={'collision': {'facecolor': 'green'}})\n",
    "draw_object(sg_2, draw_params={'collision': {'facecolor': 'red'}})\n",
    "plt.autoscale()\n",
    "plt.axis('equal')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4: Time-variant Obstacles\n",
    "\n",
    "So far, we have only considered static objects. We can also create time-varying obstacles. Note that the collision checks are only performed at discrete points in time k."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# create a new time-variant collision objects which starts at time step 1\n",
    "tvo_1 = pycrcc.TimeVariantCollisionObject(1)\n",
    "# we need to add the shape of the object for each time step\n",
    "tvo_1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 2.0, 5)) # time step 1\n",
    "tvo_1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 2.5, 5)) # time step 2\n",
    "tvo_1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 3, 5))   # time step 3\n",
    "tvo_1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 3.5, 5)) # time step 4\n",
    "tvo_1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 4, 5))   # time step 5\n",
    "tvo_1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 4.5, 5)) # time step 6\n",
    "tvo_1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 5, 5))   # time step 7\n",
    "tvo_1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 5.5, 5)) # time step 8\n",
    "\n",
    "# create a second time-variant collision objects which starts at time step 4\n",
    "tvo_2 = pycrcc.TimeVariantCollisionObject(4)\n",
    "tvo_2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 0)) # time step 4\n",
    "tvo_2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 2)) # time step 5\n",
    "tvo_2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 3)) # time step 6\n",
    "tvo_2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 4)) # time step 7\n",
    "tvo_2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 5)) # time step 8\n",
    "tvo_2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 6)) # time step 9\n",
    "tvo_2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 7)) # time step 10\n",
    "\n",
    "# Check if both objects collide\n",
    "print('Collision between time-varying obstacle tvo_1 and tvo_2: ', tvo_1.collide(tvo_2))\n",
    "\n",
    "plt.figure(figsize=(10, 10))\n",
    "draw_object(tvo_1, draw_params={'collision': {'facecolor': 'red'}})\n",
    "draw_object(tvo_2, draw_params={'collision': {'facecolor': 'green'}})\n",
    "plt.autoscale()\n",
    "plt.axis('equal')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 5: Creating a Collision Checker\n",
    "\n",
    "Several planning algorithms test a large number of candidate trajectories for collisions. These checks must be executed between each trajectory and all obstacles in the environment. The pycrcc.CollisionChecker provides the functionality to manage the set of all obstacles in the environment. After all obstacles are added to pycrcc.CollisionChecker, a collision check query for a trajectory can be called:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# create a new collision checker\n",
    "cc = pycrcc.CollisionChecker()\n",
    "# add all obstacles in the environment\n",
    "cc.add_collision_object(tvo_1)\n",
    "cc.add_collision_object(sg_1)\n",
    "cc.add_collision_object(pycrcc.RectOBB(2, 1, 1.5, 6.0, 0))\n",
    "\n",
    "print('Collision with trajectory tvo_2: ', cc.collide(tvo_2))\n",
    "\n",
    "plt.figure(figsize=(10, 10))\n",
    "draw_object(cc, draw_params={'collision': {'facecolor': 'red'}})\n",
    "draw_object(tvo_2, draw_params={'collision': {'facecolor': 'green'}})\n",
    "plt.autoscale()\n",
    "plt.axis('equal')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Sometimes, it might be necessary to get all obstacles within the collision checker at a specific point in time. This can be done with the function ‘time_slice’:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# plot all obstacles at time step 4\n",
    "plt.figure(figsize=(10, 10))\n",
    "draw_object(cc.time_slice(4), draw_params={'collision': {'facecolor': 'red'}})\n",
    "plt.autoscale()\n",
    "plt.axis('equal')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 5: Intersection Between Segments and Obstacles\n",
    "\n",
    "We can also create a segment and test it for intersections with objects in the environment."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Axis-aligned rectangle with width/2, height/2, x-position , y-position\n",
    "aabb_2 = pycrcc.RectAABB(2,3,12,8)\n",
    "\n",
    "# create collision checker and add obstacles\n",
    "cc = pycrcc.CollisionChecker()\n",
    "cc.add_collision_object(obb)\n",
    "cc.add_collision_object(aabb)\n",
    "cc.add_collision_object(aabb_2)\n",
    "cc.add_collision_object(circ)\n",
    "\n",
    "# segment with start and end point\n",
    "start = [0, 0]\n",
    "end = [9.5, 9.5]\n",
    "\n",
    "print(\"Raytrace, with join\")\n",
    "intervals = cc.raytrace(start[0], start[1], end[0], end[1], True)\n",
    "print(intervals)\n",
    "\n",
    "print(\"Raytrace, no join\")\n",
    "intervals = cc.raytrace(start[0], start[1], end[0], end[1], False)\n",
    "print(intervals)\n",
    "\n",
    "plt.figure(figsize=(10, 10))\n",
    "draw_object(cc)\n",
    "plt.axis('equal')\n",
    "plt.ylim([-5, 20])\n",
    "plt.xlim([-5, 20])\n",
    "\n",
    "# plot start and end point of segment\n",
    "plt.plot((start[0], end[0]), (start[1], end[1]), '*g', zorder=50)\n",
    "\n",
    "# plot all intersecting intervals of the segment\n",
    "for i in intervals:\n",
    "    plt.plot((i[0], i[2]), (i[1], i[3]), zorder=50)\n",
    "\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
